/**
 * @file px4ctrl_node.cpp
 * @brief Offboard control example node in POSITION control mode
 */

#include "PX4CtrlFSM.h"


/**************************** Global Variable *******************************/
ros::Time Landing_begin_time;         
ros::Time Landing_arm_time;
geometry_msgs::PoseStamped uav_ini_pose;         // initial takeoff pose of uav
geometry_msgs::PoseStamped landing_init_pose;    // initial landing pose of uav
geometry_msgs::PoseStamped offb_hoverini_pose;  // pose to js trl in offboard mode
geometry_msgs::PoseStamped offb_dockini_pose;   // pose to js trl in offboard mode


geometry_msgs::PoseStamped takeoff_pose;
geometry_msgs::PoseStamped landing_pose;
geometry_msgs::PoseStamped offb_hover_pose;   // pose to hovering in offboard mode
geometry_msgs::PoseStamped offb_docking_pose; // pose to docking  in offboard mode

// init down pose
geometry_msgs::PoseStamped landing_pose_last;
geometry_msgs::PoseStamped down_pose; // 预计爬升的高度
geometry_msgs::PoseStamped down_pose_last; //上一次爬升设定高度
geometry_msgs::PoseStamped landing_process1_pose;

// init climb pose
offboard::PosVelAcc pva_climb;
geometry_msgs::PoseStamped climb_pose; // 预计爬升的高度
geometry_msgs::PoseStamped climb_pose_last; //上一次爬升设定高度

MatrixXd Landing_detect; // landing detection
int Landing_detect_num = 8;

int ctrl_rate;
float hover_height;
float err_x;
float err_y;
float err_z;

double r_circle;
double t_total;
double omege;
int count_circle = 0;
int count_total;
bool repeat_path = false;
int circle_num = 0;
int target_circle_num;
/**************************** Function Declaration and Definition *******************************/
// Tool Function
template <typename T>
void readParam(ros::NodeHandle &nh, std::string param_name, T &loaded_param)
{
    // template to read param from roslaunch
    const std::string &node_name = ros::this_node::getName();
    param_name = node_name + "/" + param_name;
    if (!nh.getParam(param_name, loaded_param))
    {
        ROS_WARN_STREAM("Fail to load " << param_name << ", use default value:" << loaded_param);
    }
    else
    {
        ROS_INFO_STREAM("Load successfully " << param_name << ": " << loaded_param);
    }
}

void loadRosParams(ros::NodeHandle &nh)
{
    readParam<int>(nh, "ctrl_rate", ctrl_rate);
    readParam<float>(nh, "hover_height", hover_height);
    readParam<bool>(nh, "repeat_path", repeat_path);
    readParam<double>(nh, "r_circle", r_circle);
    readParam<double>(nh, "t_total", t_total);
    readParam<int>(nh, "target_circle_num", target_circle_num);

    readParam<float>(nh, "err_x", err_x);
    readParam<float>(nh, "err_y", err_y);
    readParam<float>(nh, "err_z", err_z);
}

/**************************************** Main loop *******************************************/
int main(int argc, char **argv)
{
    ros::init(argc, argv, "px4ctrl_node");
    ros::NodeHandle nh;
    // load param
    loadRosParams(nh);
    // the setpoint publishing rate MUST be faster than 2Hz
    ros::Rate ctrl_loop(ctrl_rate);
    // init class PX4CtrlFSM
    PX4CtrlFSM px4_ctrl(nh);
    px4_ctrl.loadParameters(nh);

    /// subscriber
    ros::Subscriber uav_state_sub =
        nh.subscribe<mavros_msgs::State>("/mavros/state",
                                         10,
                                         boost::bind(&PX4CtrlFSM::uav_state_cb, &px4_ctrl, _1));
    ros::Subscriber uav_pose_sub =
        nh.subscribe<geometry_msgs::PoseStamped>("/ctrl_pose/pose",
                                         10,
                                         boost::bind(&PX4CtrlFSM::uav_pose_cb, &px4_ctrl, _1));    
    ros::Subscriber uav_poseOdom_sub =
        nh.subscribe<nav_msgs::Odometry>("/ctrl_pose/odom",
                                         10,
                                         boost::bind(&PX4CtrlFSM::uav_poseOdom_cb, &px4_ctrl, _1));                      
    ros::Subscriber rc_in_sub =
        nh.subscribe<mavros_msgs::RCIn>("/mavros/rc/in",
                                         10,
                                         boost::bind(&PX4CtrlFSM::rc_in_cb, &px4_ctrl, _1));
    // ros::Subscriber euler_in_sub =
    //     nh.subscribe<geometry_msgs::PoseStamped>("/euler/IMU_data",
    //                                      10,
    //                                      boost::bind(&PX4CtrlFSM::euler_in_cb, &px4_ctrl, _1));
    ROS_INFO("Subscriber: %s", uav_state_sub.getTopic().c_str());
    ROS_INFO("Subscriber: %s", uav_pose_sub.getTopic().c_str());
    ROS_INFO("Subscriber: %s", uav_poseOdom_sub.getTopic().c_str());
    ROS_INFO("Subscriber: %s", rc_in_sub.getTopic().c_str());

    // load RosParam
    if (!px4_ctrl.px4AttitudeCtlInitRosParam())
    {
        ROS_ERROR("px4AttitudeCtlInitRosParam failed , exit !!!");
        return 0;
    }
    ros::Duration(2).sleep();

    // wait for FCU connection
    while (ros::ok() && !px4_ctrl.uav_cur_state.connected)
    {
        ros::spinOnce();
        ROS_INFO_STREAM("wait for FCU connection");
        ctrl_loop.sleep();
    }
    ROS_INFO_STREAM("\033[33m FCU Connected!! \033[0m");

    // 获得无人机当前YAW值,并以此为目标保持,如果往复起飞,则目标仍然为第一次起飞的YAW
    px4_ctrl.initializeYaw(ctrl_loop);

    /**
     * Pre Step :
     * Send a few setpoints before starting
     * Before entering Offboard mode,
     * you must have already started streaming setpoints.
     * Otherwise the mode switch will be rejected.
     */
    ros::Time startTime = ros::Time::now();
    offboard::PosVelAcc pva_before_offb;
    for (int i = 10; ros::ok() && i > 0; --i)
    {
        ros::spinOnce();
        px4_ctrl.pose2pva(px4_ctrl.uav_cur_pose, pva_before_offb);
        px4_ctrl.assignCurrentPositionVelocityAndYaw(startTime,pva_before_offb);
        ctrl_loop.sleep();
    }

    // init bool 只有在第一次进入case运行
    bool init_takeoff = false;
    bool init_landing = false;
    bool init_hover = false;

    // Variable to track whether the drone has successfully landed
    bool landed = true;
    bool landing_timer1_started = false;
    bool landing_timer2_started = false;
    bool possible_landing = false;
    bool ground_contact = false;
    bool takeoff_flag = false;

    float landing_duration = 0.30;
    float possible_landing_duration = 0.30;
    // float LNDMC_Z_POS_MAX = 0.001;
    float LNDMC_Z_POS_MAX = 0.03;
    float LNDMC_Z_VEL_MAX = 0.025;
    float LNDMC_XY_VEL_MAX = 0.025;

    // circle param
    omege = 2 * M_PI / t_total;
    count_circle = 0;
    count_total = int(ctrl_rate * t_total);

    offboard::PosVelAcc pva_offb_hover;
    px4_ctrl.pose2pva(px4_ctrl.uav_cur_pose, pva_offb_hover);

    // // 判断当前有无目标位置传入，若没有，则等待，不需要计算期望姿态
    // px4_ctrl.waitForTargetPosition(ctrl_loop);
    /**************************** main ctrl loop *******************************/
    while (ros::ok())
    {
        ros::spinOnce();
        // TODO：我们每隔五秒去检查一下与飞控的连接等是否一切正常。

        if (px4_ctrl.uav_cur_state.armed)
        {
            if (px4_ctrl.uav_cur_state.mode == "OFFBOARD")
            {
                ROS_INFO_STREAM_ONCE("\033[33m In OFFBOARD Mode \033[0m");

                // switch modes: default start mode is MODE4: IDLE
                ROS_WARN("flight_mode: %s", px4_ctrl.flightModeToString(px4_ctrl.flight_mode).c_str());

                // ============ MODE1: AUTO_TAKOFF --- condition : last_mode = IDLE && rc_in_takeoff
                if (px4_ctrl.rc_data.takeoff && px4_ctrl.flight_mode == PX4CtrlFSM::IDLE) {
                    px4_ctrl.flight_mode = PX4CtrlFSM::TAKEOFF;
                    init_hover = false;
                    init_landing = false;
                    px4_ctrl.init_dock = false;

                    landed = false;
                    landing_timer1_started = false;
                    landing_timer2_started = false;
                    possible_landing = false;
                    ground_contact = false;
                    takeoff_flag = false;
                    px4_ctrl.rc_feed = {0.0, 0.0, 0.0};
                    px4_ctrl.landing_process1_flag = false;


                // ============ MODE2: AUTO_HOVER
                } else if (px4_ctrl.rc_data.hover && !px4_ctrl.rc_data.docking 
                           && px4_ctrl.flight_mode != PX4CtrlFSM::IDLE
                           && px4_ctrl.flight_mode != PX4CtrlFSM::HOVER && !landed) {
                    px4_ctrl.flight_mode = PX4CtrlFSM::HOVER;
                    px4_ctrl.init_dock = false;
                    init_landing = false;
                    possible_landing = false;
                    ROS_WARN("------------ RC RESET ------------");
                    px4_ctrl.rc_feed = {0.0, 0.0, 0.0};
                    px4_ctrl.landing_process1_flag = false;


                // ============ MODE3: AUTO_DOCKING
                } else if (px4_ctrl.rc_data.docking && px4_ctrl.flight_mode != PX4CtrlFSM::IDLE
                           && px4_ctrl.flight_mode == PX4CtrlFSM::HOVER && !landed) {
                    px4_ctrl.flight_mode = PX4CtrlFSM::DOCKING;
                    init_hover = false;
                    init_landing = false;
                    possible_landing = false;
                    ROS_WARN("------------ RC RESET ------------");
                    px4_ctrl.rc_feed = {0.0, 0.0, 0.0};
                    px4_ctrl.landing_process1_flag = false;


                // ============ MODE3=4: AUTO_LANDING --- condition : last_mode = not IDLE && rc_in_landing
                } else if (px4_ctrl.rc_data.landing && px4_ctrl.flight_mode != PX4CtrlFSM::IDLE 
                           && px4_ctrl.flight_mode != PX4CtrlFSM::LANDING && !landed) {
                    px4_ctrl.flight_mode = PX4CtrlFSM::LANDING;
                    init_hover = false;
                    init_takeoff = false;
                    px4_ctrl.init_dock = false; 

                    ROS_WARN("------------ RC RESET ------------");
                    px4_ctrl.rc_feed = {0.0, 0.0, 0.0};


                // ============ MODE5: IDLE
                } else if ((px4_ctrl.rc_data.landing || px4_ctrl.rc_data.hover) && landed) {
                    px4_ctrl.flight_mode = PX4CtrlFSM::IDLE;
                }

                // Based on the current flight mode, execute corresponding logic
                switch (px4_ctrl.flight_mode) {
                    // =============================== TAKEOFF CASE =========================== //
                    case PX4CtrlFSM::TAKEOFF:{
                        ROS_INFO_STREAM_ONCE("\033[33m Take off start \033[0m");

                        // ============ TAKEOFF STEP1 : init - 在一次周期中 LANDING之后 往复起飞时重置
                        // get initial pose uav_ini_pose (x0,y0,z0) 
                        for (int i = 10; !init_takeoff && i > 0; --i)
                        {
                            uav_ini_pose =  px4_ctrl.uav_cur_pose;
                            px4_ctrl.pose2pva(uav_ini_pose, pva_offb_hover);
                            // 获得无人机当前YAW值,并以此为目标保持,如果往复起飞,则目标为起飞初始化的YAW
                            // px4_ctrl.initializeYaw(ctrl_loop);
                            ROS_INFO("[Initial takeoff]: Ini xyz: %.3f, %.3f, %.3f",
                                        uav_ini_pose.pose.position.x, uav_ini_pose.pose.position.y, uav_ini_pose.pose.position.z);
                            ros::spinOnce();
                            ctrl_loop.sleep();
                            
                        }
                        init_takeoff = true;

                        // ============ TAKEOFF STEP2 : climb - 可以在此阶段切入 hover - takeoff
                        takeoff_pose.pose.position.x = uav_ini_pose.pose.position.x;
                        takeoff_pose.pose.position.y = uav_ini_pose.pose.position.y;
                        takeoff_pose.pose.position.z = uav_ini_pose.pose.position.z + hover_height;

                        climb_pose.pose.position.z = px4_ctrl.uav_cur_pose.pose.position.z;
                        climb_pose_last = climb_pose;

                        double err = 0.04;
                        while (!takeoff_flag && fabs(takeoff_pose.pose.position.z - px4_ctrl.uav_cur_pose.pose.position.z) > err)
                        {
                            ros::spinOnce();

                            // 如果 px4_ctrl.rc_data.takeoff 变为 false，立即退出循环
                            if (!px4_ctrl.rc_data.takeoff)
                            {
                                ROS_WARN_THROTTLE(1, "Takeoff mode switched!");
                                break;
                            }

                            climb_pose.pose.position.x = takeoff_pose.pose.position.x;
                            climb_pose.pose.position.y = takeoff_pose.pose.position.y;
                            climb_pose.pose.position.z = climb_pose.pose.position.z + 0.2 / (1*double(ctrl_rate)); // TODO:上升速度 0.2m/s
                            climb_pose.pose.position.z = std::min(climb_pose.pose.position.z, takeoff_pose.pose.position.z);   //恒定增量总值不要超过最终目标点，防止超调。
                            climb_pose.pose.position.z = std::max(climb_pose.pose.position.z, climb_pose_last.pose.position.z); //设定的目标点不要降低，最少也要保持和上次一样。不能随当前值降低而降低，防止在低处震荡。
                            px4_ctrl.pose2pva(climb_pose, pva_climb);
                            px4_ctrl.assignCurrentPositionVelocityAndYaw(startTime,pva_climb);
                            // pubPx4Attitude.publish(px4_ctrl.msgTargetAttitudeThrust);
                            ROS_INFO("[Takeoff]: Target xyz: %.3f, %.3f, %.3f | Ini xyz: %.3f, %.3f, %.3f",
                                    climb_pose.pose.position.x, climb_pose.pose.position.y, climb_pose.pose.position.z,
                                    uav_ini_pose.pose.position.x, uav_ini_pose.pose.position.y, uav_ini_pose.pose.position.z);
                            ROS_INFO("           Curr   xyz: %.3f, %.3f, %.3f | Err: %.3f, %.3f, %.3f",
                                    px4_ctrl.uav_cur_pose.pose.position.x, px4_ctrl.uav_cur_pose.pose.position.y, px4_ctrl.uav_cur_pose.pose.position.z,
                                    takeoff_pose.pose.position.x - px4_ctrl.uav_cur_pose.pose.position.x,
                                    takeoff_pose.pose.position.y - px4_ctrl.uav_cur_pose.pose.position.y,
                                    takeoff_pose.pose.position.z - px4_ctrl.uav_cur_pose.pose.position.z);
                            ROS_INFO("rc_feedX: %.3f, rc_feedY: %.3f, rc_feedZ: %.3f", px4_ctrl.rc_feed.x, px4_ctrl.rc_feed.y, px4_ctrl.rc_feed.z);
                            ctrl_loop.sleep();
                            climb_pose_last = climb_pose;
                        }
                        ROS_INFO_STREAM_ONCE("\033[33m Take off done \033[0m");
                        takeoff_flag = true;

                        // ============ TAKEOFF STEP3 : hover - 可以在此阶段切入 hover - takeoff
                        ROS_WARN_THROTTLE(1, "=============== Finish Takeoff, Keep the state ... ===============");
                        px4_ctrl.pose2pva(takeoff_pose, pva_offb_hover);
                        px4_ctrl.assignCurrentPositionVelocityAndYaw(startTime,pva_offb_hover);
                        ROS_INFO("[Takeoff Done]: Target xyz: %.3f, %.3f, %.3f | Ini xyz: %.3f, %.3f, %.3f",
                                takeoff_pose.pose.position.x, takeoff_pose.pose.position.y, takeoff_pose.pose.position.z,
                                uav_ini_pose.pose.position.x, uav_ini_pose.pose.position.y, uav_ini_pose.pose.position.z);
                        ROS_INFO("                Curr   xyz: %.3f, %.3f, %.3f | Err: %.3f, %.3f, %.3f",
                                px4_ctrl.uav_cur_pose.pose.position.x, px4_ctrl.uav_cur_pose.pose.position.y, px4_ctrl.uav_cur_pose.pose.position.z,
                                takeoff_pose.pose.position.x - px4_ctrl.uav_cur_pose.pose.position.x,
                                takeoff_pose.pose.position.y - px4_ctrl.uav_cur_pose.pose.position.y,
                                takeoff_pose.pose.position.z - px4_ctrl.uav_cur_pose.pose.position.z);
                        ROS_INFO("rc_feedX: %.3f, rc_feedY: %.3f, rc_feedZ: %.3f", px4_ctrl.rc_feed.x, px4_ctrl.rc_feed.y, px4_ctrl.rc_feed.z);

                        }
                        break;

                    // =============================== HOVER CASE =========================== //
                    case PX4CtrlFSM::HOVER:{
                        ROS_INFO_STREAM_ONCE("\033[33m Hover start \033[0m");

                        // ============ HOVER STEP1 : init - 切入hover重置
                        // get hover pose offb_hoverini_pose (xi,yi,zi) 
                        for (int i = 10; !init_hover && i > 0; --i)
                        {
                            offb_hover_pose = px4_ctrl.uav_cur_pose;
                            offb_hoverini_pose = px4_ctrl.uav_cur_pose;
                            px4_ctrl.pose2pva(offb_hover_pose, pva_offb_hover);
                            px4_ctrl.assignCurrentPositionVelocityAndYaw(startTime,pva_offb_hover);
                            ROS_INFO("[Initial Hovering]: Target xyz: %.3f, %.3f, %.3f | Curr xyz: %.3f, %.3f, %.3f",
                                        offb_hover_pose.pose.position.x, offb_hover_pose.pose.position.y, offb_hover_pose.pose.position.z,
                                        px4_ctrl.uav_cur_pose.pose.position.x, px4_ctrl.uav_cur_pose.pose.position.y, px4_ctrl.uav_cur_pose.pose.position.z);
                            ros::spinOnce();
                            ctrl_loop.sleep();
                            
                        }
                        init_hover = true;

                        ROS_INFO_STREAM_ONCE("\033[33m In Circle mode, start circling! \033[0m");
                        if (count_circle < count_total + 1)
                        {
                            offboard::PosVelAcc pva_traj;
                            pva_traj.header.frame_id = "global";
                            pva_traj.header.stamp = ros::Time::now();
                            px4_ctrl.targetPoseLastTimeStamp = ros::Time::now().toSec();
                            double phi = double(count_circle) / double(ctrl_rate) * omege;

                            /**
                             *  case 4 circling WITH yaw rotating,
                             *         specify velocity and accleration
                             */
                            // pva_traj.px = r_circle * sin(phi) + (offb_hoverini_pose.pose.position.x);
                            // pva_traj.py = r_circle * (1 - cos(phi)) + (offb_hoverini_pose.pose.position.y);
                            // pva_traj.pz = offb_hoverini_pose.pose.position.z;
                            // pva_traj.vx = r_circle * omege * cos(phi);
                            // pva_traj.vy = r_circle * omege * sin(phi);
                            // pva_traj.vz = 0;
                            // pva_traj.ax = -r_circle * pow(omege, 2) * sin(phi);
                            // pva_traj.ay = r_circle * pow(omege, 2) * cos(phi);
                            // pva_traj.az = 0;
                            // pva_traj.yaw = 0.0;

                            // /**
                            //  *  case 1 circling WITHOUT yaw rotating
                            //  */
                            pva_traj.px = r_circle * (1 - cos(phi)) + (offb_hoverini_pose.pose.position.x);
                            pva_traj.py = -r_circle * sin(phi) + (offb_hoverini_pose.pose.position.y);
                            pva_traj.pz = offb_hoverini_pose.pose.position.z;
                            pva_traj.vx = r_circle * omege * sin(phi);
                            pva_traj.vy = -r_circle * omege * cos(phi);
                            pva_traj.vz = 0;
                            pva_traj.ax = r_circle * pow(omege, 2) * cos(phi);
                            pva_traj.ay = r_circle * pow(omege, 2) * sin(phi);
                            pva_traj.az = 0;
                            pva_traj.yaw = 0.0;

                            // 如果 px4_ctrl.rc_data.landing 变为 false，立即退出循环
                            if (!px4_ctrl.rc_data.hover)
                            {
                                ROS_WARN_THROTTLE(1, "Circling mode switched!");
                                // landed = true;
                                break;
                            }

                            px4_ctrl.assignCurrentPositionVelocityAndYaw(startTime,pva_traj);
                            offb_hover_pose = px4_ctrl.uav_cur_pose; // TODO: the pose for hovering can be the last target pose
                            count_circle++;

                            ROS_INFO("[Circleing]: Target xyz: %.3f, %.3f, %.3f | rc_feed xyz: %.3f, %.3f, %.3f",
                                    pva_traj.px, pva_traj.py, pva_traj.pz,
                                    px4_ctrl.rc_feed.x, px4_ctrl.rc_feed.y, px4_ctrl.rc_feed.z);
                            ROS_INFO("            Curr   xyz: %.3f, %.3f, %.3f | Err: %.3f, %.3f, %.3f",
                                    px4_ctrl.uav_cur_pose.pose.position.x, px4_ctrl.uav_cur_pose.pose.position.y, px4_ctrl.uav_cur_pose.pose.position.z,
                                    pva_traj.px - px4_ctrl.uav_cur_pose.pose.position.x,
                                    pva_traj.py - px4_ctrl.uav_cur_pose.pose.position.y,
                                    pva_traj.pz - px4_ctrl.uav_cur_pose.pose.position.z);
                        }
                        else
                        {
                            if (repeat_path && circle_num < (target_circle_num - 1))
                            {
                                ROS_WARN("=============== Repeat the trajectory ===============");
                                count_circle = 0; // 从头开始 pva模式不要重复路径
                                circle_num = circle_num +1;
                            }
                            else
                            {
                                ROS_WARN_THROTTLE(1, "=============== Trajectory finished, Hover ... ===============");
                                px4_ctrl.pose2pva(offb_hover_pose, pva_offb_hover);
                                px4_ctrl.assignCurrentPositionVelocityAndYaw(startTime,pva_offb_hover);
                            }
                        }
                        }
                        break;
                    
                    // =============================== DOCKING CASE =========================== //
                    case PX4CtrlFSM::DOCKING:{
                        ROS_INFO_STREAM_ONCE("\033[33m Docking start \033[0m");

                        // ============ DOCKING STEP1 : init - 切入Docking重置
                        // get Docking pose offb_dockini_pose (xi,yi,zi) 
                        for (int i = 10; !px4_ctrl.init_dock && i > 0; --i)
                        {
                            offb_docking_pose = px4_ctrl.uav_cur_pose;
                            offb_dockini_pose = px4_ctrl.uav_cur_pose;
                            px4_ctrl.offb_dockini_ori.pose.orientation = px4_ctrl.uav_cur_poseOdom.pose.pose.orientation;
                            px4_ctrl.pose2pva(offb_docking_pose, pva_offb_hover);
                            px4_ctrl.assignCurrentPositionVelocityAndYaw(startTime,pva_offb_hover);
                            ROS_INFO("[Initial Docking]: Target xyz: %.3f, %.3f, %.3f | Curr xyz: %.3f, %.3f, %.3f",
                                        offb_docking_pose.pose.position.x, offb_docking_pose.pose.position.y, offb_docking_pose.pose.position.z,
                                        px4_ctrl.uav_cur_pose.pose.position.x, px4_ctrl.uav_cur_pose.pose.position.y, px4_ctrl.uav_cur_pose.pose.position.z);
                            ros::spinOnce();
                            ctrl_loop.sleep();
                            
                        }
                        px4_ctrl.init_dock = true;

                        // Currently, js input will not change the target 
                        // ============ DOCKING STEP2 : init - 切入Docking重置
                        offb_docking_pose.pose.position.x = offb_dockini_pose.pose.position.x;
                        offb_docking_pose.pose.position.y = offb_dockini_pose.pose.position.y;
                        offb_docking_pose.pose.position.z = offb_dockini_pose.pose.position.z;

                        px4_ctrl.pose2pva(offb_docking_pose, pva_offb_hover);
                        px4_ctrl.assignCurrentPositionVelocityAndYaw(startTime,pva_offb_hover);
                        ROS_INFO("[Docking]: Target xyz: %.3f, %.3f, %.3f | rc_feed xyz: %.3f, %.3f, %.3f",
                                offb_docking_pose.pose.position.x, offb_docking_pose.pose.position.y, offb_docking_pose.pose.position.z,
                                px4_ctrl.rc_feed.x, px4_ctrl.rc_feed.y, px4_ctrl.rc_feed.z);
                        ROS_INFO("            Curr   xyz: %.3f, %.3f, %.3f | Err: %.3f, %.3f, %.3f",
                                px4_ctrl.uav_cur_pose.pose.position.x, px4_ctrl.uav_cur_pose.pose.position.y, px4_ctrl.uav_cur_pose.pose.position.z,
                                offb_docking_pose.pose.position.x - px4_ctrl.uav_cur_pose.pose.position.x,
                                offb_docking_pose.pose.position.y - px4_ctrl.uav_cur_pose.pose.position.y,
                                offb_docking_pose.pose.position.z - px4_ctrl.uav_cur_pose.pose.position.z);
                        }
                        break;

                    // =============================== LANDING CASE =========================== //
                    case PX4CtrlFSM::LANDING:{
                        ROS_INFO_STREAM_ONCE("\033[33m LANDING start \033[0m");

                        // ============ LANDING STEP1 : init - 在相信人操控的前提下,此时在当前位置悬停,并获取降落位置
                        // get landing pose landing_init_pose (xd,yd,zd) 
                        for (int i = 10; !init_landing && i > 0; --i) 
                        {
                            landing_pose = px4_ctrl.uav_cur_pose;
                            landing_init_pose = px4_ctrl.uav_cur_pose;
                            px4_ctrl.pose2pva(landing_pose, pva_offb_hover);
                            px4_ctrl.assignCurrentPositionVelocityAndYaw(startTime,pva_offb_hover);
                            // pubPx4Attitude.publish(px4_ctrl.msgTargetAttitudeThrust);
                            ROS_INFO("[Initial Landing]: Target xyz: %.3f, %.3f, %.3f | Curr xyz: %.3f, %.3f, %.3f",
                                    landing_pose.pose.position.x, landing_pose.pose.position.y, landing_pose.pose.position.z,
                                    px4_ctrl.uav_cur_pose.pose.position.x, px4_ctrl.uav_cur_pose.pose.position.y, px4_ctrl.uav_cur_pose.pose.position.z);
                            ros::spinOnce();
                            ctrl_loop.sleep();

                            down_pose.pose.position.z = px4_ctrl.uav_cur_pose.pose.position.z;
                            down_pose_last = down_pose;
                        }
                        init_landing = true;
                        
                        double error_lx, error_ly, error_lz;
                        Landing_detect = Eigen::MatrixXd::Constant(1, Landing_detect_num+1, px4_ctrl.uav_cur_pose.pose.position.z);

                        // ============ LANDING STEP2 : Landing process 2 - z 下降 可以动rc
                        while (!possible_landing) {

                            landing_pose.pose.position.x = landing_init_pose.pose.position.x + px4_ctrl.rc_feed.x;
                            landing_pose.pose.position.y = landing_init_pose.pose.position.y + px4_ctrl.rc_feed.y;
                            landing_pose.pose.position.z = uav_ini_pose.pose.position.z;

                            error_lx = fabs(landing_pose.pose.position.x - px4_ctrl.uav_cur_pose.pose.position.x);
                            error_ly = fabs(landing_pose.pose.position.y - px4_ctrl.uav_cur_pose.pose.position.y);
                            error_lz = fabs(landing_pose.pose.position.z - px4_ctrl.uav_cur_pose.pose.position.z);

                            // 如果 px4_ctrl.rc_data.landing 变为 false，立即退出循环
                            if (!px4_ctrl.rc_data.landing)
                            {
                                ROS_WARN_THROTTLE(1, "Landing mode switched!");
                                // landed = true;
                                break;
                            }

                            // landing_pose_last.pose.position.z = px4_ctrl.uav_cur_pose.pose.position.z;
                            Landing_detect.leftCols(Landing_detect_num)=Landing_detect.rightCols(Landing_detect_num);
                            Landing_detect(0,Landing_detect_num) = px4_ctrl.uav_cur_pose.pose.position.z;

                            down_pose.pose.position.x = landing_pose.pose.position.x;
                            down_pose.pose.position.y = landing_pose.pose.position.y;
                            down_pose.pose.position.z = down_pose.pose.position.z - 0.2 / (1 * double(ctrl_rate)); // 降落速度
                            px4_ctrl.pose2pva(down_pose, pva_offb_hover);
                            px4_ctrl.assignCurrentPositionVelocityAndYaw(startTime, pva_offb_hover);
                            ROS_INFO("[Landing process]: Target xyz: %.3f, %.3f, %.3f | rc_feed xyz: %.3f, %.3f, %.3f",
                                    down_pose.pose.position.x, down_pose.pose.position.y, down_pose.pose.position.z,
                                    px4_ctrl.rc_feed.x, px4_ctrl.rc_feed.y, px4_ctrl.rc_feed.z);
                            ROS_INFO("                     Curr   xyz: %.3f, %.3f, %.3f | Err: %.3f, %.3f, %.3f",
                                    px4_ctrl.uav_cur_pose.pose.position.x, px4_ctrl.uav_cur_pose.pose.position.y, px4_ctrl.uav_cur_pose.pose.position.z,
                                    error_lx,error_ly,error_lz);

                            ros::spinOnce();
                            ctrl_loop.sleep();

                            //触地检测
                            // STEP1 : 检测地面接触 
                            // if (fabs(px4_ctrl.uav_cur_pose.pose.position.z - uav_ini_pose.pose.position.z) < 0.025) {
                            // if ((px4_ctrl.thrustTarget < px4_ctrl.baseThrust) && 
                            //     fabs(px4_ctrl.uav_cur_pose.pose.position.z - landing_pose_last.pose.position.z) < LNDMC_Z_POS_MAX) {
                            if ((px4_ctrl.thrustTarget < px4_ctrl.baseThrust) && 
                                fabs(Landing_detect(0,Landing_detect_num) - Landing_detect(0,0)) < LNDMC_Z_POS_MAX) {

                                // Check if landing timer has not started and start it
                                if (!landing_timer1_started && !ground_contact) {
                                    Landing_begin_time = ros::Time::now();
                                    landing_timer1_started = true;
                                    ROS_WARN("=============== 1: landing_timer_started =============== ");
                                }
                                ROS_WARN("[STEP 1 : landing_time_gap]: %.3f",(ros::Time::now() - Landing_begin_time).toSec());
                                ROS_WARN(" Thrust thre err: %.6f,  POS_Z thre err: %.6f", 
                                (px4_ctrl.thrustTarget - px4_ctrl.baseThrust), fabs(Landing_detect(0,Landing_detect_num) - Landing_detect(0,0)) - LNDMC_Z_POS_MAX);
                                if ((ros::Time::now() - Landing_begin_time).toSec() > landing_duration) {
                                    ROS_WARN_THROTTLE(1, "=============== STEP 1 PASS ===============");
                                    ROS_WARN(" Thrust thre err: %.6f,  POS_Z thre err: %.6f", 
                                    (px4_ctrl.thrustTarget - px4_ctrl.baseThrust), fabs(Landing_detect(0,Landing_detect_num) - Landing_detect(0,0)) - LNDMC_Z_POS_MAX);
                                    ground_contact = true;
                                }

                            } else {
                                ground_contact = false;
                                landing_timer1_started = false;
                            }

                            // STEP 2 : 检测降落完成
                            // if (ground_contact && 
                            //     fabs(px4_ctrl.uav_cur_poseOdom.twist.twist.linear.z) < LNDMC_Z_VEL_MAX &&
                            //     fabs(px4_ctrl.uav_cur_poseOdom.twist.twist.linear.x) < LNDMC_XY_VEL_MAX &&
                            //     fabs(px4_ctrl.uav_cur_poseOdom.twist.twist.linear.y) < LNDMC_XY_VEL_MAX) 
                            if (ground_contact && 
                                (((ros::Time::now() - Landing_begin_time).toSec() >= 3.00 && (px4_ctrl.thrustTarget < 0.82 * px4_ctrl.baseThrust))
                                || (px4_ctrl.thrustTarget < 0.80 * px4_ctrl.baseThrust))) 
                                {

                                // Check if landing timer has not started and start it
                                if (!landing_timer2_started && !possible_landing) {
                                    Landing_arm_time = ros::Time::now();
                                    landing_timer2_started = true;
                                    ROS_WARN("=============== 2: landing_timer_started =============== ");
                                }
                                ROS_WARN("[STEP 2 : landing_time_gap]: %.3f",(ros::Time::now() - Landing_arm_time).toSec());
                                ROS_INFO(" VEL thre err: %.3f, %.3f, %.3f", 
                                fabs(px4_ctrl.uav_cur_poseOdom.twist.twist.linear.x) - LNDMC_XY_VEL_MAX,
                                fabs(px4_ctrl.uav_cur_poseOdom.twist.twist.linear.y) - LNDMC_XY_VEL_MAX,
                                fabs(px4_ctrl.uav_cur_poseOdom.twist.twist.linear.z) - LNDMC_Z_VEL_MAX);
                                if ((ros::Time::now() - Landing_arm_time).toSec() > possible_landing_duration) {
                                    ROS_WARN_THROTTLE(1, "=============== Finish Landing ===============");
                                    ROS_INFO(" VEL thre err: %.3f, %.3f, %.3f", 
                                    fabs(px4_ctrl.uav_cur_poseOdom.twist.twist.linear.x) - LNDMC_XY_VEL_MAX,
                                    fabs(px4_ctrl.uav_cur_poseOdom.twist.twist.linear.y) - LNDMC_XY_VEL_MAX,
                                    fabs(px4_ctrl.uav_cur_poseOdom.twist.twist.linear.z) - LNDMC_Z_VEL_MAX);
                                    landed = true;
                                    possible_landing = true;
                                    break;
                                }

                            } else {
                                landing_timer2_started = false;
                                possible_landing = false;
                            }
                        }

                        }
                        break;
                    case PX4CtrlFSM::IDLE:{
                        // IDLE mode：Thrust = 0
                        offboard::PosVelAcc pva_curr;
                        px4_ctrl.pose2pva(px4_ctrl.uav_cur_pose, pva_curr);
                        px4_ctrl.assignCurrentPositionVelocityAndYaw(startTime,pva_curr);
                        }
                        break;
                    
                }

                // manual rc switch to postion mode
            }
            // not offboard mode: pub will no work for ctrl
            else
            {
                // ROS_WARN_THROTTLE(1, "waiting for Offboard cmd");
                ROS_WARN("NOT IN Offboard cmd");
                offboard::PosVelAcc pva_curr;
                px4_ctrl.pose2pva(px4_ctrl.uav_cur_pose, pva_curr);
                px4_ctrl.assignCurrentPositionVelocityAndYaw(startTime,pva_curr);
                // pubPx4Attitude.publish(px4_ctrl.msgTargetAttitudeThrust);
                // init_takeoff = false;
                // init_landing = false;
                // init_hover = false;
                // landed = true;
                // landing_timer_started = false;
            }

        }
        // not armed: pub will no work for ctrl
        else
        {
            // ROS_WARN_THROTTLE(1, " Initialize flags, and waiting for Vehicle arm");
            ROS_WARN("waiting for Vehicle arm");
            offboard::PosVelAcc pva_curr;
            px4_ctrl.pose2pva(px4_ctrl.uav_cur_pose, pva_curr);
            px4_ctrl.assignCurrentPositionVelocityAndYaw(startTime,pva_curr);
            // pubPx4Attitude.publish(px4_ctrl.msgTargetAttitudeThrust);
            init_takeoff = false;
            init_landing = false;
            init_hover = false;
            px4_ctrl.init_dock = false;

            landed = true;
            landing_timer1_started = false;
            landing_timer2_started = false;
            possible_landing = false;
            ground_contact = false;
            takeoff_flag = false;
        }

        ctrl_loop.sleep();
        // 判断当前有无目标位置传入，若没有，则等待，不需要计算期望姿态
        px4_ctrl.waitForTargetPosition(ctrl_loop);
    }

    return 0;
}
